#include <stdint.h>
#include "pid.h"
#include "motor.h"
#include "Encoder.h"

// PID控制器结构体
typedef struct {
    float kp;           // 比例系数
    float ki;           // 积分系数
    float kd;           // 微分系数
    float target;       // 目标值
    float actual;       // 实际值
    float error;        // 当前误差
    float last_error;   // 上次误差
    float sum_error;    // 累计误差(积分项)
    float delta_error;  // 误差变化(微分项)
    float output;       // 输出值
    float output_max;   // 输出上限
    float output_min;   // 输出下限
    uint8_t initialized; // 是否已初始化
} PID_TypeDef;

// 四个电机的PID控制器
static PID_TypeDef motor_pid[4];

// 初始化PID控制器
void PID_Init(uint8_t motor_id, float kp, float ki, float kd, float output_max, float output_min)
{
    if (motor_id >= 1 && motor_id <= 4) {
        uint8_t index = motor_id - 1;
        motor_pid[index].kp = kp;
        motor_pid[index].ki = ki;
        motor_pid[index].kd = kd;
        motor_pid[index].target = 0;
        motor_pid[index].actual = 0;
        motor_pid[index].error = 0;
        motor_pid[index].last_error = 0;
        motor_pid[index].sum_error = 0;
        motor_pid[index].delta_error = 0;
        motor_pid[index].output = 0;
        motor_pid[index].output_max = output_max;
        motor_pid[index].output_min = output_min;
        motor_pid[index].initialized = 1;
    }
}

// 设置PID目标值
void PID_SetTarget(uint8_t motor_id, float target)
{
    if (motor_id >= 1 && motor_id <= 4) {
        motor_pid[motor_id - 1].target = target;
    }
}

// PID计算并输出结果
float PID_Calculate(uint8_t motor_id, float actual)
{
    if (motor_id < 1 || motor_id > 4 || !motor_pid[motor_id - 1].initialized) {
        return 0;
    }
    
    uint8_t index = motor_id - 1;
    PID_TypeDef *pid = &motor_pid[index];
    
    // 更新实际值和误差
    pid->actual = actual;
    pid->last_error = pid->error;
    pid->error = pid->target - pid->actual;
    
    // 计算积分项和微分项
    pid->sum_error += pid->error;
    pid->delta_error = pid->error - pid->last_error;
    
    // 防止积分饱和，根据实际输出范围调整积分上限
    if (pid->sum_error > 300) pid->sum_error = 300;
    if (pid->sum_error < -300) pid->sum_error = -300;
    
    // 计算PID输出
    pid->output = pid->kp * pid->error + 
                  pid->ki * pid->sum_error + 
                  pid->kd * pid->delta_error;
    
    // 限制输出范围
    if (pid->output > pid->output_max) {
        pid->output = pid->output_max;
    }
    else if (pid->output < pid->output_min) {
        pid->output = pid->output_min;
    }
    
    return pid->output;
}

// 使用编码器数据更新所有电机PID控制
void PID_UpdateAllMotors(int16_t motor1_rpm, int16_t motor2_rpm, int16_t motor3_rpm, int16_t motor4_rpm)
{
    int32_t pwm_value;
    
    // 更新电机1
    if (motor_pid[0].initialized) {
        pwm_value = (int32_t)PID_Calculate(1, (float)motor1_rpm);
        Motor_SetSpeed(1, pwm_value);
    }
    
    // 更新电机2
    if (motor_pid[1].initialized) {
        pwm_value = (int32_t)PID_Calculate(2, (float)motor2_rpm);
        Motor_SetSpeed(2, pwm_value);
    }
    
    // 更新电机3
    if (motor_pid[2].initialized) {
        pwm_value = (int32_t)PID_Calculate(3, (float)motor3_rpm);
        Motor_SetSpeed(3, pwm_value);
    }
    
    // 更新电机4
    if (motor_pid[3].initialized) {
        pwm_value = (int32_t)PID_Calculate(4, (float)motor4_rpm);
        Motor_SetSpeed(4, pwm_value);
    }
}

// 主PID控制循环函数，使用编码器直接计算并更新
void PID_Control(int16_t *motor1_rpm, int16_t *motor2_rpm, int16_t *motor3_rpm, int16_t *motor4_rpm)
{
   
    // 获取当前所有电机速度
    Encoder_Calculate_RPM(motor1_rpm, motor2_rpm, motor3_rpm, motor4_rpm);
    
    // 使用PID更新所有电机
    PID_UpdateAllMotors(*motor1_rpm, *motor2_rpm, *motor3_rpm, *motor4_rpm);
}

// 获取PID参数函数
float PID_GetKp(uint8_t motor_id)
{
    if (motor_id >= 1 && motor_id <= 4) {
        return motor_pid[motor_id - 1].kp;
    }
    return 0.0f;
}

float PID_GetKi(uint8_t motor_id)
{
    if (motor_id >= 1 && motor_id <= 4) {
        return motor_pid[motor_id - 1].ki;
    }
    return 0.0f;
}

float PID_GetKd(uint8_t motor_id)
{
    if (motor_id >= 1 && motor_id <= 4) {
        return motor_pid[motor_id - 1].kd;
    }
    return 0.0f;
}

float PID_GetTarget(uint8_t motor_id)
{
    if (motor_id >= 1 && motor_id <= 4) {
        return motor_pid[motor_id - 1].target;
    }
    return 0.0f;
}

float PID_GetActual(uint8_t motor_id)
{
    if (motor_id >= 1 && motor_id <= 4) {
        return motor_pid[motor_id - 1].actual;
    }
    return 0.0f;
}

float PID_GetError(uint8_t motor_id)
{
    if (motor_id >= 1 && motor_id <= 4) {
        return motor_pid[motor_id - 1].error;
    }
    return 0.0f;
}

float PID_GetOutput(uint8_t motor_id)
{
    if (motor_id >= 1 && motor_id <= 4) {
        return motor_pid[motor_id - 1].output;
    }
    return 0.0f;
}

// 设置PID参数函数
void PID_SetKp(uint8_t motor_id, float kp)
{
    if (motor_id >= 1 && motor_id <= 4) {
        motor_pid[motor_id - 1].kp = kp;
    }
}

void PID_SetKi(uint8_t motor_id, float ki)
{
    if (motor_id >= 1 && motor_id <= 4) {
        motor_pid[motor_id - 1].ki = ki;
    }
}

void PID_SetKd(uint8_t motor_id, float kd)
{
    if (motor_id >= 1 && motor_id <= 4) {
        motor_pid[motor_id - 1].kd = kd;
    }
}

void PID_ALLinit(void)
{
    PID_Init(1, 0.649f, 0.153f, 0.053f, 80, 0); // A电机：进一步降低Kp和Ki，增加Kd以平滑启动
    PID_Init(2, 0.649f, 0.152f, 0.053f, 80, 0); // B电机：同上
    PID_Init(3, 0.649f, 0.152f, 0.053f, 80, 0); // C电机：同上
    PID_Init(4, 0.649f, 0.152f, 0.053f, 80, 0); // D电机：同上
}
